﻿/**
 * MOTOR 2 - RAPID FLASH DYNAMICS
 * Copyright (c) 2006-2008 Michael Baczynski http://www.polygonal.de
 * 
 * Based on Box2D by Erin Catto, http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
package de.polygonal.motor2
{
	//TODO destroy body not working properly!
	
	import de.polygonal.motor2.collision.nbody.*;
	import de.polygonal.motor2.collision.shapes.ShapeSkeleton;
	import de.polygonal.motor2.dynamics.Island;
	import de.polygonal.motor2.dynamics.RigidBody;
	import de.polygonal.motor2.dynamics.RigidBodyData;
	import de.polygonal.motor2.dynamics.contact.Contact;
	import de.polygonal.motor2.dynamics.contact.ContactManager;
	import de.polygonal.motor2.dynamics.contact.ContactNode;
	import de.polygonal.motor2.dynamics.forces.Buoyancy;
	import de.polygonal.motor2.dynamics.forces.ForceGenerator;
	import de.polygonal.motor2.dynamics.forces.ForceRegistry;
	import de.polygonal.motor2.math.AABB2;
	import de.polygonal.motor2.math.V2;	

	public class World
	{
		public static var stats_SepAxisQueryCount:int = 0;
		public static var stats_timeSimStep:int = 0;
		
		public static var doPositionCorrection:Boolean = true;
		public static var doWarmStarting:Boolean = true; 
		
		public var worldAABB:AABB2;
		public var groundBody:RigidBody;
		
		public var doSleep:Boolean;
		
		public var bodyList:RigidBody;
		
		public var bodyDestroyList:RigidBody;
		
		public var contactCount:int;
		public var contactList:Contact;
		
		public const gravity:V2 = new V2();
		
//		public var jointList:Joint;
		public var jointCount:int;
		
		private var _numBodies:int;
		private var _numContacts:int;
		private var _numJoints:int;
		
		private var _contactManager:ContactManager;
		private var _broadphase:IBroadPhase;
		private var _forceRegistry:ForceRegistry;
		private var _island:Island;
		
		private var _bodyIdCount:int;
		
		public function World(worldAABB:AABB2, doSleep:Boolean)
		{
			this.worldAABB = worldAABB.copy();
			this.doSleep = doSleep;
			
			init();
		}
		
		public function init():void
		{
			bodyList        = null;
			bodyDestroyList = null;
			contactList     = null;
//TODO joints	jointList       = null;
			
			_numBodies   = 0;
			_numContacts = 0;
			_numJoints   = 0;
			
			setGravity(0, 100);
			
			_contactManager = new ContactManager(this);
			_forceRegistry = new ForceRegistry();
			_island = new Island();
			
			_bodyIdCount = 0;
			
			setBroadPhase(new ExhaustiveSearch());
			
			//TODO worldAABB
			//TODO groundBody
			//_bidQue = new ArrayedQueue(Constants.k_maxProxies);
			//for (var i:int = 0; i < Constants.k_maxProxies; i++) _bidQue.enqueue(i+1);
		}
		
		public function deconstruct():void
		{
			destroyBody(groundBody);
			_broadphase = null;
		}
		
		/**
		 * Defines the world's gravity.
		 */
		public function setGravity(x:Number, y:Number):void
		{
			gravity.x = x;
			gravity.y = y;
		}
		
		//max shapecount = maxbodies * maxShapesPerbody!
		//public function nextBodyId(id:int):void
		//{
			//_bidQue.enqueue(id);
		//}
		/*public function dequeueProxyId():int
		{
			return _bidQue.dequeue();
		}*/
		
		/**
		 * The world provides a single ground body with no collision shapes.
		 * You can use this to simplify the creation of joints.
		 */
		public function getGroundBody():RigidBody
		{
			//TODO return groundBody
			return null;
		}
		
		public function getWorldBounds():AABB2
		{
			//TODO	getWorldBounds
			return null;
		}
		
		public function setWorldBounds(bounds:AABB2):void
		{
			//TODO setWorldBounds
		}

		/**
		 * Returns an instance to the currently used broad-phase algorithm.
		 */
		public function getBroadPhase():IBroadPhase
		{
			return _broadphase;
		}
		
		/**
		 * Assigns a new broad-phase strategy; this is only possible if the
		 * world doesn't contain any bodies.
		 */
		public function setBroadPhase(broadPhase:IBroadPhase):void
		{
			if (bodyList) return;
			
			_broadphase = broadPhase;
			_broadphase.setWorld(this);
			
			//TODO use through world.getWorldBounds
			_broadphase.setWorldBounds(worldAABB);
			_broadphase.setPairHandler(_contactManager);
		}
		
		/**
		 * Adds a new rigid body to the world.
		 */
		public function createBody(data:RigidBodyData):RigidBody
		{
			var b:RigidBody = new RigidBody(this, data);
			b.id = _bodyIdCount++;
			
			b.next = bodyList;
			if (bodyList) bodyList.prev = b;
			bodyList = b;
			_numBodies++;
			return b;
		}
		
		/**
		 * Applies a force to a rigid body.
		 */
		public function addForce(body:RigidBody, fg:ForceGenerator):Boolean
		{
			if (body == null) return false;
			
			//build triangle list
			if (fg is Buoyancy)
			{
				var s:ShapeSkeleton = body.shapeList;
				while (s)
				{
					s.triangulate();
					s = s.next;
				}
			}
			//TODO check for duplicates
			_forceRegistry.add(body, fg);
			return true;
		}
		
		/**
		 * Removes a force from a rigid body.
		 */
		public function removeForce(body:RigidBody, fg:ForceGenerator):Boolean
		{
			return _forceRegistry.remove(body, fg);
		}
		
		/**
		 * Destroys a given rigid body; destruction is deferred to make contact
		 * processing more robust.
		 */
		public function destroyBody(b:RigidBody):void
		{
			if (b.stateBits & RigidBody.k_bitDestroy) return;
			
			//TODO recycle bid
			//restoreShapeId, done in Shape deconstructor()?
			//var s:ShapeSkeleton = b.shapeList;
			//while (s)
			//{
				//_bidQue.enqueue(s.proxyId);
				//s = s.next;
			//}
			
			if (b.prev) b.prev.next = b.next;
			if (b.next) b.next.prev = b.prev;
			if (b == bodyList) bodyList = b.next;
			
			b.stateBits |= RigidBody.k_bitDestroy;
			if (_numBodies > 0) --_numBodies;
			
			//add to deferred destruction list
			b.prev = null;
			b.next = bodyDestroyList;
			bodyDestroyList = b;
		}
		
		/**
		 * Performs a simulation step.
		 */
		public function step(dt:Number, iterations:int):void
		{
			var b:RigidBody, c:Contact;
			var i:int;
			
			//handle deferred contact destruction
			_contactManager.cleanContactList();
			
			//handle deferred body destruction
			cleanBodyList();
			
			/*/////////////////////////////////////////////////////////
			// CREATE AND/OR UPDATE CONTACTS
			/////////////////////////////////////////////////////////*/
			
			_contactManager.collide();
			
			/*/////////////////////////////////////////////////////////
			// BUILD AND SIMULATE ALL AWAKE ISLANDS
			/////////////////////////////////////////////////////////*/
			
			//clear all island flags set during DFS traversal
			b = bodyList;
			while (b)
			{
				b.stateBits &= ~0x20;
				b = b.next;
			}
			c = contactList;
			while (c)
			{
				c.stateBits &= ~Contact.k_bitIsland; //0x20
				c = c.next;
			}
			
			//TOOD joints
			/*
			var j:Joint = jointList;
			while (j)
			{
				j.fIsland = false;
				j = j.next;
			}*/
			
			/*/////////////////////////////////////////////////////////
			// evaluate all attached forces
			/////////////////////////////////////////////////////////*/
			
			_forceRegistry.evaluate();
			
			var stack:Array = [];
			var stackSize:int;
			
			var other:RigidBody;			
			var cn:ContactNode;
//			var jn:JointNode;

			var seed:RigidBody;
			for (seed = bodyList; seed != null; seed = seed.next)
			{
				if (seed.stateBits & RigidBody.k_bitExclude)
					continue;
				
				//reset island and stack.
				_island.clear();
				stack[0] = seed;
				stackSize = 1;
				
				//mark node
				seed.stateBits |= 0x20;
				
				//perform a depth first search (DFS) on the constraint graph.
				//http://lab.polygonal.de/2007/06/13/data-structures-example-the-graph-class/
				while (stackSize > 0)
				{
					//grab the next body off the stack and add it to the island.
					b = stack[--stackSize];
					_island.addBody(b);
					
					//make sure the body is awake.
					b.stateBits &= ~RigidBody.k_bitSleep;
					
					//to keep islands as small as possible, we don't propagate
					//islands across static bodies.
					if (b.stateBits & RigidBody.k_bitStatic)
						continue;
					
					//search all contacts connected to this body
					for (cn = b.contactList; cn; cn = cn.next)
					{
						//already marked
						if (cn.contact.stateBits & Contact.k_bitIsland)
							continue;
						
						//add and mark
						_island.addContact(cn.contact);
						cn.contact.stateBits |= Contact.k_bitIsland;
						
						//other body marked
						other = cn.other;
						if (other.stateBits & 0x20)
							continue;
						
						stack[stackSize++] = other;
						other.stateBits |= 0x20;
					}
					
					/*TODO joints
					//search all joints connected to this body
					for (jn = b.jointList; jn; jn = jn.next)
					{
						var joint:Joint = jn.joint;
						if (joint.islandFlag) continue;
						island.addJoint(joint);
						joint.islandFlag = true;
						
						other = jn.other;
						if (other.stateBits & 0x20)
							continue;
						
						stack[stackSize++] = other;
						other.stateBits |= 0x20;
					}*/  
				}
				
				_island.solve(gravity.x, gravity.y, iterations, dt);
				if (doSleep) _island.updateSleep(dt);
				
				//post solve cleanup
				for (i = 0; i < _island.bodyCount; i++)
				{
					b = _island.bodies[i];
					
					//allow static bodies to participate in other islands
					if (b.stateBits & RigidBody.k_bitStatic)
						b.stateBits &= ~0x20;
				
					//handle newly frozen bodies.
					if (b.stateBits & RigidBody.k_bitFrozen)
					{
						//TODO define callback method world.notifyBoundaryCallback
						//or dispatch NotifyBoundaryViolated() event
						/*var response:uint = m_listener.NotifyBoundaryViolated(b);
						if (response == b2WorldListener.b2_destroyBody)
						{
							destroyBody(b);
							b = null;
							island.bodies[i] = null;
						}*/
					}
				}
			}
			
			_broadphase.search();
			
			//flush pair buffer
			//_broadphase.flush();
		}
		
		//TODO cleanBodyList()
		public function cleanBodyList():void
		{
			_contactManager.destroyImmediate = true;
			
			//TODO distribute destruction
			/*
			if (bodyDestroyList)
			{
				var b:RigidBody = bodyDestroyList;
				bodyDestroyList = b.next;
				
				b.deconstruct();
				b = null; 	
			}*/
			
			var b:RigidBody = bodyDestroyList;
			while (b)
			{
				var t:RigidBody = b;
				b = b.next;
				
				//TODO joints needed.
				//Delete the attached joints
				/* JOINTS
				b2JointNode* jn = b0->m_jointList;
				while (jn)
				{
					b2JointNode* jn0 = jn;
					jn = jn->next;
					if (m_listener)
						m_listener->NotifyJointDestroyed(jn0->joint);
					DestroyJoint(jn0->joint);
				}*/
				
				t.deconstruct();
				t = null;
			}
			
			bodyDestroyList = null;
			
			_contactManager.destroyImmediate = false;
		}
	}
}
